using System;
using Microsoft.SPOT;
using NetduinoHappyHour.Hardware;
using NetduinoHappyHour.Util;
using SecretLabs.NETMF.Hardware;
using SecretLabs.NETMF.Hardware.Netduino;
using System.Collections;

namespace NetduinoHappyHour.Behaviour
{
    /// <summary>
    /// makes the robot strolling around, triing not to bump against any objects
    /// </summary>
    public class Explore : BaseBehaviour, IExecutable
    {
        
        public void Execute()
        {
            DriveCommand driveStraight = new DriveCommand(10, 10, SystemConfig.Instance().DelayStepper);
            ISensor ultrasonicSensorL = SensorFactory.Instance("NetduinoHappyHour.Hardware.Sharp2d120x", Pins.GPIO_PIN_A0);
            double objDistanceL;
            ISensor ultrasonicSensorR = SensorFactory.Instance("NetduinoHappyHour.Hardware.Sharp2d120x", Pins.GPIO_PIN_A1);
            double objDistanceR;

            bool runExplorer = true;

            /// Due to the usage of stepper engines, I decided not to use asynchronuous sensor reading.
            /// If asynchronuous reading would required, timers would be fine.
            while (runExplorer)
            {
                objDistanceL = ultrasonicSensorL.GetDistance();
                objDistanceR = ultrasonicSensorR.GetDistance();
                if (objDistanceL != 1L && objDistanceL < 8 || objDistanceR != 1L && objDistanceR < 8)
                {
                    // just for debugging
                    new DisplayMessage("L: " + objDistanceL + " R: " + objDistanceR).Execute();
                    // avoid colission by stop
                    runExplorer = false;
                }
                else if (objDistanceL != 1L && objDistanceL < 16 || objDistanceL != 1L && objDistanceL < 16)
                {
                    // just for debugging
                    new DisplayMessage("L: " + objDistanceL + " R: " + objDistanceR).Execute();
                    // evasion maneuver
                    gw.ExecuteDriveCommand(GetEvasionManeuver(objDistanceL, objDistanceR));
                }
                else
                {
                    gw.ExecuteDriveCommand(driveStraight);
                }
            }
        }

        /// <summary>
        /// TODO improve avoiding
        /// </summary>
        /// <param name="sensorData">Input Data to create appropriate evasion maneuver</param>
        /// <returns></returns>
        private DriveCommand GetEvasionManeuver(double ObjDistanceL, double ObjDistanceR)
        {
            // if object in left areas rotate right
            if (ObjDistanceL > ObjDistanceR)
            {
                return new DriveCommand(SystemConfig.Instance().Rotate90 / 2, -SystemConfig.Instance().Rotate90 / 2, SystemConfig.Instance().DelayStepper);
            }
            else
            {
                return new DriveCommand(-SystemConfig.Instance().Rotate90 / 2, SystemConfig.Instance().Rotate90 / 2, SystemConfig.Instance().DelayStepper);
            }
        }
    }
}
